#ifndef MISCFUNCTIONS_H
#define MISCFUNCTIONS_H

#include "DataStructures.h"
#include <opencv2/core/core.hpp>

// interesting depth camera hacks
namespace idch {

// computes the distance point 'p' is from the line defined by 'begin' and 'end'
// does not bound the line by the end points of the line segment it is defined from
double lineDistance2( const cv::Point &begin ,
                      const cv::Point &end ,
                      const cv::Point &p );

template <class T>
T lineDistance2( const T &lineX,
                      const T &lineY,
                      const T &slopeX,
                      const T &slopeY,
                      const T &pointX,
                      const T &pointY )
{
    // find the closest point on the line.
    T a,b;
    closestPoint(lineX,lineY,slopeX,slopeY,pointX,pointY,a,b);

    // compute distance from closest point
    T dx = pointX-a;
    T dy = pointY-b;

    return dx*dx + dy*dy;
}

template <class T>
void closestPoint( const T &lineX,
                   const T &lineY,
                   const T &slopeX,
                   const T &slopeY,
                   const T &pointX,
                   const T &pointY,
                   T &closestX ,
                   T &closestY )
{
    // compute the location on the line which is closest
    T t = slopeX*(pointX-lineX)+slopeY*(pointY-lineY);
    t /= slopeX*slopeX + slopeY*slopeY;

    // find the closest point on the line
    closestX = slopeX*t + lineX;
    closestY = slopeY*t + lineY;
}

template <class T>
double distance( const T& x0 , const T& y0 , const T& x1 , const T& y1 )
{
    double dx = x1-x0;
    double dy = y1-y0;

    return sqrt(dx*dx + dy*dy);
}

// computes the acute angle between two vectors
template <class T>
double acuteAngle( T x0 , T y0 , T x1 , T y1 )
{
    double n0 = sqrt( x0*x0 + y0*y0 );
    double n1 = sqrt( x1*x1 + y1*y1 );

    return acos((x0*x1 + y0*y1)/(n0*n1));
}

// intersection of two line segments
void findIntersection( const LineSegmentF &a , const LineSegmentF &b , float &pt_x , float &pt_y );

double acuteAngle( const LineSegmentF& a , const LineSegmentF& b );

// returns the acute angle between vector a to b ant a to c
double acuteAngle( const cv::Point2f &a , const cv::Point2f &b , const cv::Point2f &c );

// Creates a RectPt by finding intersections of the line sections.
RectPt convertRect( const LineSegRect& orig );

void convertGray16to8( const cv::Mat& src , const cv::Mat& dst , uint16_t maxValue );

void saveCalibration( const StereoCalib& calib , int imgWidth , int imgHeight );

}

#endif // MISCFUNCTIONS_H
